#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/core/core.hpp>

using namespace cv;
using namespace std;

void findmatch(const Mat &image1, const Mat &image2, vector<KeyPoint> &keypoints1, 
               vector<KeyPoint> &keypoints2, vector<DMatch> &goodmatches)
{
    Mat descriptors1,descriptors2;
    Ptr<FeatureDetector> detector=ORB::create();
    Ptr<DescriptorExtractor> descriptor=ORB::create();
    Ptr<DescriptorMatcher> matcher=DescriptorMatcher::create("BruteForce-Hamming");
    
    detector->detect(image1, keypoints1);
    detector->detect(image2, keypoints2);
    
    descriptor->compute(image1,keypoints1,descriptors1);
    descriptor->compute(image2,keypoints2,descriptors2);
    
    vector<DMatch> matches;
    matcher->match(descriptors1,descriptors2,matches);
    
    auto min_max=minmax_element(matches.begin(), matches.end(),[](DMatch &m1, DMatch &m2){
        return m1.distance>m2.distance;
    });
    
    double max_dist=min_max.first->distance;
    double min_dist=min_max.second->distance;
    
    for(int i=0; i<matches.size();i++){
        if(matches[i].distance<max(2*min_dist,30.0)){
            goodmatches.push_back(matches[i]);
        }
    }
}

void getmatchpoint(const vector<KeyPoint> &keypoints1, const vector<KeyPoint> &keypoints2, 
                   const vector<DMatch> &goodmatches, vector<Point2f> &arraypoint1, vector<Point2f> &arraypoint2)
{
    for(int i=0; i<goodmatches.size();i++){
        arraypoint1.push_back(keypoints1[goodmatches[i].queryIdx].pt);
        arraypoint2.push_back(keypoints2[goodmatches[i].trainIdx].pt);
    }
}

void getcameraK(const double &focal_length, const double &alph, 
                const double &imagewidth, const double &imagehight,
               Mat &K) 
{
    K=(Mat_<double>(3,3)<<focal_length, 0, imagewidth/2, 0, alph*focal_length, imagehight/2, 0,0,1);
}

void gettransmat(const vector<Point2f> &points1, const vector<Point2f> &points2, const Mat &K,
                 Mat &Fun, Mat &Ess, Mat &R, Mat &t)
{
    Fun=findFundamentalMat(points1, points2, CV_FM_8POINT);
    
    double focal_length=K.at<double>(0,0);
    Point2f principalpoint;
    principalpoint.x=K.at<double>(0,2);
    principalpoint.y=K.at<double>(1,2);
    Ess=findEssentialMat(points1, points2, focal_length, principalpoint);
    
    recoverPose(Ess, points1, points2, R, t, focal_length, principalpoint);
}

Point3f reproject(const cv::Point2f& point1, const cv::Point2f& point2, const Mat &R1,const Mat &t1,
          const Mat &R2, const Mat &t2, const Mat &K)
{
   
    cv::Mat cposture1(3,4,CV_64F);
    hconcat(R1, t1, cposture1);

    cv::Mat cposture2(3,4,CV_64F);
    hconcat(R2, t2, cposture2);
    
    cv::Mat M1=K*cposture1;
    cv::Mat M2=K*cposture2;
    
    cv::Mat A(4,3,CV_64F);
    cv::Mat b(4,1,CV_64F);
    
    A.at<double>(0,0)=M1.at<double>(0,0)-M1.at<double>(2,0)*point1.x;
    A.at<double>(0,1)=M1.at<double>(0,1)-M1.at<double>(2,1)*point1.x;
    A.at<double>(0,2)=M1.at<double>(0,2)-M1.at<double>(2,2)*point1.x;
    b.at<double>(0,0)=M1.at<double>(2,3)*point1.x-M1.at<double>(0,3);
    
    A.at<double>(1,0)=M1.at<double>(1,0)-M1.at<double>(2,0)*point1.y;
    A.at<double>(1,1)=M1.at<double>(1,1)-M1.at<double>(2,1)*point1.y;
    A.at<double>(1,2)=M1.at<double>(1,2)-M1.at<double>(2,2)*point1.y;
    b.at<double>(1,0)=M1.at<double>(2,3)*point1.y-M1.at<double>(1,3);
    
    A.at<double>(2,0)=M2.at<double>(0,0)-M2.at<double>(2,0)*point2.x;
    A.at<double>(2,1)=M2.at<double>(0,1)-M2.at<double>(2,1)*point2.x;
    A.at<double>(2,2)=M2.at<double>(0,2)-M2.at<double>(2,2)*point2.x;
    b.at<double>(2,0)=M2.at<double>(2,3)*point2.x-M2.at<double>(0,3);
    
    A.at<double>(3,0)=M2.at<double>(0,0)-M2.at<double>(2,0)*point2.y;
    A.at<double>(3,1)=M2.at<double>(0,1)-M2.at<double>(2,1)*point2.y;
    A.at<double>(3,2)=M2.at<double>(0,2)-M2.at<double>(2,2)*point2.y;
    b.at<double>(3,0)=M2.at<double>(2,3)*point2.y-M2.at<double>(0,3);
    
    cv::Mat XYZ(3,1,CV_64F);
    cv::solve(A,b,XYZ,DECOMP_SVD);
    cv::Point3f worldp;
    worldp.x=XYZ.at<double>(0,0);
    worldp.y=XYZ.at<double>(1,0);
    worldp.z=XYZ.at<double>(2,0);
    return worldp;
}

vector<Point3f> getWpoints3d(const vector<Point2f> &points1, const vector<Point2f> points2, 
                          const Mat &R1,const Mat &t1, const Mat &R2,const Mat &t2,
                          const Mat &K)
{
    vector<Point3f> wpoints;
    Point3f worldp;
    for(int i=0; i<points1.size();++i){
    worldp=reproject(points1[i],points2[i],R1,t1, R2, t2,K);
    wpoints.push_back(worldp);
    }
    return wpoints;
}
    

int main(int argc, char **argv) 
{
    Mat image1=imread("../1.jpg", IMREAD_ANYCOLOR);
    Mat image2=imread("../2.jpg", IMREAD_ANYCOLOR);
    assert(image1.data!=nullptr&&image2.data!=nullptr);
    
    double focallength, alph, imagewidth, imagehight;
    Mat K;
    focallength=1220.0;
    alph=1;
    imagewidth=1980;
    imagehight=1080;
    getcameraK(focallength, alph, imagewidth, imagehight,K);
    
    vector<KeyPoint> keypoints1, keypoints2;
    vector<DMatch> goodmatches;
    findmatch(image1, image2, keypoints1, keypoints2, goodmatches);
    cout<<goodmatches.size()<<endl;
    
    vector<Point2f> points1, points2;
    getmatchpoint(keypoints1, keypoints2, goodmatches, points1, points2);
    
    Mat Fun, Ess, R21, t21;
    gettransmat(points1, points2, K, Fun, Ess, R21, t21);
    cout<<R21<<endl;
    cout<<t21<<endl;
    Mat R11=(Mat_<double>(3,3)<<1,0,0,0,1,0,0,0,1);
    Mat t11=(Mat_<double>(3,1)<<0,0,0);
    
    vector<Point3f> worldpoint;
    worldpoint=getWpoints3d(points1, points2, R11, t11, R21, t21,K);
    cout<<worldpoint.size()<<endl;
    for(int i=0; i<worldpoint.size(); i++){
        if(worldpoint[i].z>0)
        cout<<worldpoint[i].x<<" "<<worldpoint[i].y<<" "<<worldpoint[i].z<<endl;
    }
    return 0;
}
